
#include <pob-eye.h>


/** Distance sensor parameter :
 * 
 * Number after the symbol name are the PORTA's pin of your distance sensor.
 *
 * DISTANCE_SENSOR_FRONT : sensor at the front of the pob-bot
 *
 *
 */
#define DISTANCE_SENSOR_FRONT 5
#define DISTANCE_SENSOR_TOURETTE 3

/**
*DISTANCE_WARNING :
* 
**/
#define DISTANCE_WARNING 72 


static void InitPobProto (void);
static void MoveAndStop(UInt8 Way,UInt32 time);
static void MoveBot(UInt8 Way);

//move order for pob-proto board
#define MOVE_RUN		0x05
#define MOVE_BACK		0x0A
#define MOVE_LEFT		0x09
#define MOVE_RIGHT		0x06
#define STOP_BOT		0


int main( void )
{
	int sensorFront ;
	int sensorTourette ;
	
	int moveOrder ;
	int botBlocked = 0 ;
	
	InitPOBEYE();
	InitPobProto();
	
	PobProto Proto ;

	while(1)
	{
		//get sensor value
		//sensorFront = GetPortAnalog(DISTANCE_SENSOR_FRONT);
		
		sensorTourette = GetPortAnalog(DISTANCE_SENSOR_TOURETTE);
		
		//PrintTextOnPobTerminal("sensorFront = %d " , sensorFront);
		
		PrintTextOnPobTerminal("sensorTourette = %d " , sensorTourette);
		
		//move bot :

		moveOrder = STOP_BOT ;

		if( sensorTourette < DISTANCE_WARNING)
		{
			moveOrder = MOVE_RUN ;
		}
		else
		{
			MoveBot(MOVE_BACK);
			Wait(20000);
			botBlocked == 1;
			
		}
		
		MoveBot(moveOrder);
		botBlocked = 0;
	
	}
	return 0;
}


/**
 * Initialize the POB-PROTO board. PORTA set as analogic, PORTC as servomotor.
 */
void InitPobProto (void)
{
	PobProto board;
	

	//to get the position of the analogic joystick, you have to set the PORTA as analogic input
	board.porta=ALL_PORTA_AS_ANA;	

	//all pin of PORTC are configured to manage servomotors
	board.portc=RC7_AS_SERVO |RC6_AS_SERVO |RC3_AS_SERVO |RC2_AS_SERVO |RC1_AS_SERVO |RC0_AS_SERVO;
	

	//RD0 RD1 RD2 RD3 are configured as digitals output to gear DC motor
	//RD4 RD5 RD6 RD7 are configured as digitals input
	board.portd=RD7_AS_DI |RD6_AS_DI |RD5_AS_DI |RD4_AS_DI |RD3_AS_DO |RD2_AS_DO |RD1_AS_DO |RD0_AS_DO;
	
	//set the pob proto
	SetPobProto(&board);
}

/**
 * Move and stop bot.
 *
 * @param Way : move order
 * @param time : time to move
 */
void MoveAndStop( UInt8 Way, UInt32 time )
{
	MoveBot(Way);
	Wait(time);
	MoveBot(STOP_BOT);
}

/**
 * Move the bot
 *
 * @param Way : move bot order
 */
void MoveBot (UInt8 Way)
{	
	SetPortD(Way);
}
